
//
// Created by deng on 2022/12/25.
//
#include "common_inc.h"
#include "configurations.h"
#include "mpu6500.h"
#include "encoder_calibrator_stm32.h"
#include <bsp_can.h>
#include <cstdlib>
#include "stdio.h"


#if  defined(Driver_IMU6)|| defined(Driver_IRC)
#include "bsp_colorLED.h"
#endif

extern Motor motor;
extern EncoderCalibrator encoderCalibrator;
extern bool FindedHomeFlag,FindHomeFlagUpdataFlag,FindHomeNeedReverseFlag;

CAN_TxHeaderTypeDef txHeader =
        {
                .StdId = 0x00,
                .ExtId = 0x00,
                .IDE = CAN_ID_STD,
                .RTR = CAN_RTR_DATA,
                .DLC = 8,
                .TransmitGlobalTime = DISABLE
        };

void BackRunSta(uint8_t* data){
    static float tmpF;
    tmpF = motor.controller->GetPosition();
    auto* b = (unsigned char*) &tmpF;
    for (int i = 0; i < 4; i++)
        data[i] = *(b + i);

    data[4] = motor.controller->state == (Motor::STATE_FINISH ? 1 : 0) | (motor.encoder->angleData.rectifyValid ? 1 : 0) << 1 | (FindedHomeFlag ? 1 : 0) << 2 | motor.controller->state == (Motor::STATE_OVERLOAD ? 1 : 0) << 3;

    txHeader.StdId = (boardConfig.canNodeId << 7) | 0x23;
    CAN_Send(&txHeader, data);
}


void OnCanCmd(uint8_t _id,uint8_t _cmd, uint8_t* _data, uint32_t _len)
{
    static float tmpF;
    static int32_t tmpI;
    static short shortTem;

    if(FindedHomeFlag == false && _cmd == 7){
        if(_id == boardConfig.canNodeId)
            BackRunSta(_data);
        return;
    }

    switch (_cmd)
    {
        // 0x00~0x0F No Memory CMDs
        case 0x01:  // Enable Motor
            if(*(uint32_t*) (RxData) == 1){
                if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
                {
                    motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                    motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
                }
                motor.controller->SetPositionSetPoint(motor.controller->GetPosition());
            }else{
                motor.controller->requestMode = Motor::MODE_STOP;
            }

            break;
        case 0x02:  // Do Calibration
            encoderCalibrator.isTriggered = true;
            break;
        case 0x03:  // Set Current SetPoint
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_CURRENT)
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_CURRENT);
            motor.controller->SetCurrentSetPoint((int32_t) (*(float*) RxData * 1000));
            break;
        case 0x04:  // Set Velocity SetPoint
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_VELOCITY)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_VELOCITY);
            }
            motor.controller->SetVelocitySetPoint(
                    (int32_t) (*(float*) RxData *
                               (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            break;
        case 0x05:  // Set Position SetPoint
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
            }
            motor.controller->SetPositionSetPoint(
                    (int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            if (_data[4]) // Need Position & Finished ACK
            {
                if(_id == boardConfig.canNodeId)
                    BackRunSta(_data);
            }
            break;
        case 0x06:  // Set Position with Time

            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
            motor.controller->SetPositionSetPointWithTime(
                    (int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS),
                    *(float*) (RxData + 4));
            if (_data[4]) // Need Position & Finished ACK
            {
                if(_id == boardConfig.canNodeId)
                    BackRunSta(_data);
            }
            break;
        case 0x07:  // Set Position with Velocity-Limit


            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
            }
            motor.config.motionParams.ratedVelocity =
                    (int32_t) (*(float*) (RxData + 4) * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);

            motor.controller->SetPositionSetPoint(
                    (int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));

//             Always Need Position & Finished ACK
            if(_id == boardConfig.canNodeId)
                BackRunSta(_data);

            break;
        case 0x08:
#if  defined(Driver_IMU6)|| defined(Driver_IRC)
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
                motor.controller->requestMode = Motor::MODE_COMMAND_POSITION;

            motor.config.motionParams.ratedVelocity =
                    (int32_t) (*(float*)(RxData+4) * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);

            motor.controller->SetPositionSetPoint(
                    (int32_t) (*(float*)RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            if(_id == boardConfig.canNodeId)
                BackRunSta(_data);
#endif
            break;
        case 0x09:
#if  defined(Driver_IMU6)|| defined(Driver_IRC)

            if(_id == boardConfig.canNodeId)
                BackRunSta(_data);
#endif
            break;

            // 0x10~0x1F CMDs with Memory
        case 0x11:  // Set Node-ID and Store to EEPROM

            boardConfig.canNodeId = *(uint32_t*) (RxData);
            if (_data[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x12:  // Set Current-Limit and Store to EEPROM
            if(*(float*) RxData > 0 && *(float*) RxData < 3){
                motor.config.motionParams.ratedCurrent = (int32_t) (*(float*) RxData * 1000);
                boardConfig.currentLimit = motor.config.motionParams.ratedCurrent;
                if (_data[4])
                    boardConfig.configStatus = CONFIG_COMMIT;
            }
            *(float*)_data = (float)motor.config.motionParams.ratedCurrent/1000;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x12;
            CAN_Send(&txHeader, _data);
            break;
        case 0x13:  // Set Velocity-Limit and Store to EEPROM
            motor.config.motionParams.ratedVelocity =
                    (int32_t) (*(float*) RxData *
                               (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
            boardConfig.velocityLimit = motor.config.motionParams.ratedVelocity;
            if (_data[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x14:  // Set Acceleration （and Store to EEPROM）
            tmpF = *(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;

            motor.config.motionParams.ratedVelocityAcc = (int32_t) tmpF;
            motor.motionPlanner.velocityTracker.SetVelocityAcc((int32_t) tmpF);
            motor.motionPlanner.positionTracker.SetVelocityAcc((int32_t) tmpF);
            boardConfig.velocityAcc = motor.config.motionParams.ratedVelocityAcc;
            if (_data[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x15:  // Apply Home-Position and Store to EEPROM
            motor.controller->ApplyPosAsHomeOffset();
            boardConfig.encoderHomeOffset = motor.config.motionParams.encoderHomeOffset %
                                            motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
            boardConfig.configStatus = CONFIG_COMMIT;

            *(int32_t*)_data = boardConfig.encoderHomeOffset;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x15;
            CAN_Send(&txHeader, _data);
            break;
        case 0x16:  // Set Auto-Enable and Store to EEPROM
            boardConfig.enableMotorOnBoot = (*(uint32_t*) (RxData) == 1);
            if (_data[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x17:  // Set DCE Kp
            if(*(int32_t *) (RxData) != 0) {
                motor.config.ctrlParams.dce.kp = *(int32_t*) (RxData);
                boardConfig.dce_kp = motor.config.ctrlParams.dce.kp;
            }
            //if (_data[4])
            //   boardConfig.configStatus = CONFIG_COMMIT;
            *(uint32_t*)_data = motor.config.ctrlParams.dce.kp;
            // Finished ACK
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x17;
            CAN_Send(&txHeader, _data);
            break;
        case 0x18:  // Set DCE Kv
            if(*(int32_t *) (RxData) != 0) {
                motor.config.ctrlParams.dce.kv = *(int32_t *) (RxData);
                boardConfig.dce_kv = motor.config.ctrlParams.dce.kv;
            }
            //if (_data[4])
            //    boardConfig.configStatus = CONFIG_COMMIT;
            *(uint32_t*)_data = motor.config.ctrlParams.dce.kv;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x18;
            CAN_Send(&txHeader, _data);
            break;
        case 0x19:  // Set DCE Ki
            if(*(int32_t *) (RxData) != 0) {
                motor.config.ctrlParams.dce.ki = *(int32_t *) (RxData);
                boardConfig.dce_ki = motor.config.ctrlParams.dce.ki;
            }
            //if (_data[4])
            //    boardConfig.configStatus = CONFIG_COMMIT;
            *(uint32_t*)_data = motor.config.ctrlParams.dce.ki;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x19;
            CAN_Send(&txHeader, _data);
            break;
        case 0x1A:  // Set DCE Kd
            if(*(int32_t *) (RxData) != 0) {
                motor.config.ctrlParams.dce.kd = *(int32_t *) (RxData);
                boardConfig.dce_kd = motor.config.ctrlParams.dce.kd;
            }
            //if (_data[4])
            //    boardConfig.configStatus = CONFIG_COMMIT;
            *(uint32_t*)_data = motor.config.ctrlParams.dce.kd;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x1A;
            CAN_Send(&txHeader, _data);
            break;
        case 0x1B:  // Set Enable Stall-Protect
            motor.config.ctrlParams.stallProtectSwitch = (*(uint32_t*) (RxData) == 1);
            boardConfig.enableStallProtect = motor.config.ctrlParams.stallProtectSwitch;
            if (_data[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;


            // 0x20~0x2F Inquiry CMDs
        case 0x21: // Get Current
        {
            tmpF = motor.controller->GetFocCurrent();
            auto* b = (unsigned char*) &tmpF;
            for (int i = 0; i < 4; i++)
                _data[i] = *(b + i);
            _data[4] = (motor.controller->state == Motor::STATE_FINISH ? 1 : 0);

            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x21;
            CAN_Send(&txHeader, _data);
        }
            break;
        case 0x22: // Get Velocity
        {
            tmpF = motor.controller->GetVelocity();
            auto* b = (unsigned char*) &tmpF;
            for (int i = 0; i < 4; i++)
                _data[i] = *(b + i);
            _data[4] = (motor.controller->state == Motor::STATE_FINISH ? 1 : 0);

            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x22;
            CAN_Send(&txHeader, _data);
        }
            break;
        case 0x23: // Get Position
        {
            if(_id == boardConfig.canNodeId)
                BackRunSta(_data);
        }
            break;
        case 0x24: // Get Offset
        {
            tmpI = motor.config.motionParams.encoderHomeOffset;
            auto* b = (unsigned char*) &tmpI;
            for (int i = 0; i < 4; i++)
                _data[i] = *(b + i);
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x24;
            CAN_Send(&txHeader, _data);
        }
            break;

        case 0x31:  // Set DCE Kd
            if(*(int16_t *)&RxData[0] != 0 || *(int16_t *)&RxData[2] != 0 || *(int16_t *)&RxData[4] != 0 || *(int16_t *)&RxData[6] != 0 ) {
                motor.config.ctrlParams.dce.kp = *(uint16_t *)&RxData[0];
                boardConfig.dce_kp = motor.config.ctrlParams.dce.kp;
                motor.config.ctrlParams.dce.ki = *(uint16_t *)&RxData[2];
                boardConfig.dce_ki = motor.config.ctrlParams.dce.ki;
                motor.config.ctrlParams.dce.kd = *(uint16_t *)&RxData[4];
                boardConfig.dce_kd = motor.config.ctrlParams.dce.kd;
                motor.config.ctrlParams.dce.kv = *(uint16_t *)&RxData[6];
                boardConfig.dce_kv = motor.config.ctrlParams.dce.kv;
            }
            //if (_data[4])
            //    boardConfig.configStatus = CONFIG_COMMIT;
            *(uint16_t*)&_data[0] = motor.config.ctrlParams.dce.kp;
            *(uint16_t*)&_data[2] = motor.config.ctrlParams.dce.ki;
            *(uint16_t*)&_data[4] = motor.config.ctrlParams.dce.kd;
            *(uint16_t*)&_data[6] = motor.config.ctrlParams.dce.kv;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x31;
            CAN_Send(&txHeader, _data);
            break;
        case 0x32:
            _data[0] = boardConfig.HardVer;
            _data[1] = 0;
            _data[2] = 0;
            _data[3] = SOFT_VER_A;
            _data[4] = SOFT_VER_B;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x32;
            CAN_Send(&txHeader, _data);
            break;
        case 0x41:
            if(FindedHomeFlag == false && (RxData[5] == FIND_HOME_WITH_PHYSICAL_LIMIT || RxData[5] == FIND_HOME_WITH_PHYSICAL_KEY || RxData[5] == FIND_HOME_WITHOUT_APPLY)){
                //motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_VELOCITY);

                if(IsCalibratorOtherKeyOn()){
                    FindHomeNeedReverseFlag = true;
                }
                if(IsCalibratorKeyOn()){
                    if(boardConfig.HardVer == BATTLE_CHARGE_THERE_AXIS)
                        FindHomeNeedBackMs = 500;
                    else if(boardConfig.HardVer == TRANSMIT_PLANE_THERE_AXIS)
                        FindHomeNeedBackMs = 500;
                    else
                        FindHomeNeedBackMs = 2000;
                }

                motor.controller->SetVelocitySetPoint(
                        (int32_t) (*(float*) RxData *
                                   (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS) * (FindHomeNeedBackMs == 0?1:-1) * (FindHomeNeedReverseFlag==false?1:-1));

                if(RxData[4] == 1)
                    FindHomeFlagUpdataFlag = true;
                else
                    FindHomeFlagUpdataFlag = false;
                motor.config.motionParams.ratedCurrent = (int32_t)RxData[6]*100;

                if(IsCalibratorKeyOn() == false){
                    NeedFindHomeFalg = (NEED_FIND_HOME_FLAG)RxData[5];
                    LastNeedFindHomeFlag = NeedFindHomeFalg;
                }
                UpdatedToFlashFlag = false;


            }else if(FindedHomeFlag == true ){
                _data[0] = true;
                *(uint16_t*)&_data[2] = boardConfig.encoderHomeOffset;
                *(uint16_t*)&_data[4] = FindHomeRealPos;
                txHeader.StdId = (boardConfig.canNodeId << 7) | 0x41;
                if(_id == boardConfig.canNodeId)
                    CAN_Send(&txHeader, _data);
                if(UpdatedToFlashFlag == false && LastNeedFindHomeFlag != FIND_HOME_WITHOUT_APPLY){
                    boardConfig.encoderHomeOffset = FindHomeRealPos;
                    //boardConfig.configStatus = CONFIG_COMMIT;
                    UpdatedToFlashFlag = true;
                }
                //if(boardConfig.canNodeId != 3)
                //    motor.controller->SetCtrlMode(Motor::MODE_STOP);
            }
//            if(motor.controller->state == Motor::STATE_OVERLOAD){
//                BackRunSta(_data);
//            }


            break;

        case 0x42:
            *(short*)&_data[0] = (short)(MpuData.axAngleMean*100);
            *(short*)&_data[2] = (short)(MpuData.ayAngleMean*100);
            *(short*)&_data[4] = (short)(MpuData.azAngleMean*100);
            *(short*)&_data[6] = (short)(MpuData.MagDirMean*100);
            //printf("axo:%.3f ax:%.3f ay:%.3f az:%.3f MagDir:%.3f\n",MpuData.ax,MpuData.axAngle,MpuData.ayAngle,MpuData.azAngle,MpuData.MagDir);
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x42;
            CAN_Send(&txHeader, _data);

            break;
        case 0x43:
            motor.controller->requestMode = Motor::MODE_STOP;
            motor.controller->ApplyHomeInOneCrile();
            FindedHomeFlag = true;
            _data[0] = true;
            *(uint16_t*)&_data[2] = boardConfig.encoderHomeOffset;
            *(uint16_t*)&_data[4] = motor.encoder->angleData.rectifiedAngle;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x43;
            CAN_Send(&txHeader, _data);


            break;

        case 0x44:
//            if(TofDis != 0xFFFFFFFF){
                *(uint32_t*)&_data[0] = TofDis;
                txHeader.StdId = (boardConfig.canNodeId << 7) | 0x44;
                CAN_Send(&txHeader, _data);
                TofDis = 0xFFFFFFFD;
//            }

            break;
        case 0x56:  // gpio write or read
        {
            if(_data[0]){
                HAL_GPIO_WritePin((GPIO_TypeDef *)(GPIOA_BASE + (GPIOB_BASE - GPIOA_BASE)*_data[1]), (uint16_t)(1<<_data[2]), _data[3] ? GPIO_PIN_SET : GPIO_PIN_RESET);
            }else{
                HAL_GPIO_ReadPin((GPIO_TypeDef *)(GPIOA_BASE + (GPIOB_BASE - GPIOA_BASE)*_data[1]), (uint16_t)(1<<_data[2]));
            }

            _data[7] = 1;
            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x56;
            CAN_Send(&txHeader, _data);
        }
        case 0x57:  // Set Position with Velocity-Limit
        {
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
            }
            motor.config.motionParams.ratedVelocity =
                    (int32_t) (*(float*) (RxData + 4) * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
            motor.controller->SetPositionSetPoint(
                    (int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            // Always Need Position & Finished ACK
            BackRunSta(_data);
        }
            break;
        case 0x7D:
            HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
            FindedHomeFlag = false;
            BackRunSta(_data);
            break;

        case 0x7e:  // Erase Configs
            boardConfig.configStatus = CONFIG_RESTORE;
            break;
        case 0x7f:  // Reboot
            HAL_NVIC_SystemReset();
            break;
        case 0x50:
#if  defined(Driver_IMU6)|| defined(Driver_IRC)

                *(uint32_t*)&_data[0] = imu6_data.Find_Home;
                *(uint32_t*)&_data[1] = imu6_data.Elector1;
                *(uint32_t*)&_data[2] = imu6_data.Elector2;
                *(uint32_t*)&_data[3] = imu6_data.Color_Flag;

            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x50;
            CAN_Send(&txHeader, _data);
#endif
            break;
        case 0x51:

#if  defined(Driver_IMU6)|| defined(Driver_IRC)
            if(boardConfig.canNodeId == 0x04){
                TCS34725_GetRawData(&rgb);  //读两次，实际测试时发现读到的颜色总是上一次的颜色
            }
            _data[0] = rgb.r>>8&0xff;
            _data[1] = rgb.r>>0&0xff;
            _data[2] = rgb.g>>8&0xff;
            _data[3] = rgb.g>>0&0xff;
            _data[4] = rgb.b>>8&0xff;
            _data[5] = rgb.b>>0&0xff;
            _data[6] = rgb.c>>8&0xff;
            _data[7] = rgb.c>>0&0xff;

            txHeader.StdId = (boardConfig.canNodeId << 7) | 0x51;
            CAN_Send(&txHeader, _data);
#endif
            break;

        default:
            break;
    }

}

